#include "arduino.h"
#include "bluetooth_ps2.h" 


uint8_t _serialRead;
uint8_t _dataLen;
uint8_t _index = 0;
uint8_t _prevc = 0;
uint8_t buffer1[20];
uint8_t buffer1_bak[20];  
uint8_t ps2_data_list[23];
uint8_t ps2_data_list_bak[23];
long _lasttime;
bool _isStart;
bool _isAvailable;
bool _isReady;

void writeBuffer(int16_t index,uint8_t c)
{
  _index=index;
  buffer1[_index]=c;
}

void readSerial(HardwareSerial &SerialX)  
{
  _isAvailable = false;
  if(SerialX.available() > 0)
  {
    _isAvailable = true;
    _serialRead = SerialX.read();
  }
}



bool readjoystick(HardwareSerial &SerialX)
{
  bool result = false;
  if(millis() - _lasttime > 200)
  {
    _isReady = false;
    _isStart=false;
    _prevc = 0x00;
    _index=0;
    buffer1[2] = buffer1[4] = buffer1[6] =buffer1[8] =buffer1[9]=0x80;
    buffer1[1] = buffer1[3] = buffer1[5] =buffer1[7] =0x00;
  }
  readSerial(SerialX);
  while(_isAvailable)
  {
    _lasttime = millis();
    unsigned char c = _serialRead & 0xff;
    if((c == 0xAA) && (_isStart == false))
    {
      if(_prevc == 0xff)
      {
        _index=1;
        _isStart = true;
        writeBuffer(_index,c);
      }
    }
    else
    {
      _prevc = c;
      if(_isStart)
      {
        writeBuffer(_index,c);
      }
    }
    _index++;
      if(_index > 12)
      {
        _index=0; 
        _isStart=false;
        _prevc = 0x00;
        _isReady=false;
        result = false;
        buffer1[2] = buffer1[4] = buffer1[6] =buffer1[8] =buffer1[9]=0x80;
        buffer1[1] = buffer1[3] = buffer1[5] =buffer1[7] =0x00;
      }
    if(_isStart && (_index > 9))
    {
      uint8_t checksum=0;
      checksum = buffer1[2]+buffer1[3]+buffer1[4]+buffer1[5]+buffer1[6]+buffer1[7]+buffer1[8];
      if(checksum == buffer1[9])
      {
        _isReady = true;
         _isStart = false;
        _index = 0;
        _prevc = 0x00;
        result = true;
        for(uint8_t i=0;i<10;i++)
        {
            buffer1_bak[i]=buffer1[i];
        }
      }
      else
      {
        _isStart = false;
        _index = 0;
        _prevc = 0x00;
        _isReady=false;
        result = false;
        buffer1[2] = buffer1[4] = buffer1[6] =buffer1[8] =buffer1[9]=0x80;
        buffer1[1] = buffer1[3] = buffer1[5] =buffer1[7] =0x00;
      }
    }
    readSerial(SerialX);
   }
   return result;
}

bool PS2ButtonPressed(uint8_t button) 
{
  if (!_isReady)
  {
    return ps2_data_list_bak[button];
  }
  else
  {
    ps2_data_list_bak[button] = ps2_data_list[button];
    return  ps2_data_list[button];

  }
}

void ps2loop(HardwareSerial &SerialX)
{
  if(readjoystick(SerialX))
  {
    ps2_data_list[HteJOYSTICK_LY] = buffer1_bak[2];
    ps2_data_list[HteJOYSTICK_LX] = buffer1_bak[4];
    ps2_data_list[HteJOYSTICK_RY] = buffer1_bak[6];
    ps2_data_list[HteJOYSTICK_RX] = buffer1_bak[8];
    ps2_data_list[HteJOYSTICK_ZR] = (buffer1_bak[3] & 0x01) == 0x01 ? true : false;
    ps2_data_list[HteJOYSTICK_R] = (buffer1_bak[3] & 0x02) == 0x02 ? true : false;
    ps2_data_list[HteJOYSTICK_ZL] = (buffer1_bak[3] & 0x04) == 0x04 ? true : false; 
    ps2_data_list[HteJOYSTICK_L] = (buffer1_bak[3] & 0x08) == 0x08 ? true : false;
    ps2_data_list[HteJOYSTICK_HOME] = (buffer1_bak[3] & 0x10) ==0x10 ? true : false;
    ps2_data_list[HteJOYSTICK_BUTTON_L] = (buffer1_bak[3] & 0x20) == 0x20 ? true : false ;

    ps2_data_list[HteJOYSTICK_Y] = (buffer1_bak[5] & 0x01) == 0x01 ? true : false;
    ps2_data_list[HteJOYSTICK_B] = (buffer1_bak[5] & 0x02) == 0x02 ? true : false;
    ps2_data_list[HteJOYSTICK_A] = (buffer1_bak[5] & 0x04) == 0x04 ? true : false;
    ps2_data_list[HteJOYSTICK_X] = (buffer1_bak[5] & 0x08) == 0x08 ? true : false;
    ps2_data_list[HteJOYSTICK_PLUS] = (buffer1_bak[5] & 0x10) == 0x10 ? true : false;
    ps2_data_list[HteJOYSTICK_MODE] = (buffer1_bak[5] & 0x20) == 0x20 ? true : false;

    ps2_data_list[HteJOYSTICK_UP] = (buffer1_bak[7] & 0x01) == 0x01 ? true : false;
    ps2_data_list[HteJOYSTICK_DOWN] = (buffer1_bak[7] & 0x02) == 0x02 ? true : false ;
    ps2_data_list[HteJOYSTICK_LEFT] = (buffer1_bak[7] & 0x04) == 0x04 ? true : false ;
    ps2_data_list[HteJOYSTICK_RIGHT] = (buffer1_bak[7] & 0x08) == 0x08 ? true : false ;
    ps2_data_list[HteJOYSTICK_MINUS] = (buffer1_bak[7] & 0x10) == 0x10 ? true : false ;
    ps2_data_list[HteJOYSTICK_BUTTON_R] = (buffer1_bak[7] & 0x20) == 0x20 ? true : false ;

  }
}
